/**
 * 
 */
package SRTPlanner;

import java.util.ArrayList;

import simulation.Configuration;
import simulation.LineSeg;
import simulation.Map;
import simulation.Obstacle;
import simulation.Point;
import simulation.Robot;

/**
 * @author aclayton
 *
 */
public class SimpleLocalPlanner {
	private Map map;
	
	
	public SimpleLocalPlanner(Map map){
		this.map = map;
	}
	
	public boolean existsPath(Configuration config1, Configuration config2){
		// TODO write local planner
		ArrayList<Robot> robots = config1.getRobots();
		
		for(Robot r : robots){
			Point p1 = config1.getRobotPosition(r).getCenter();
			Point p2 = config2.getRobotPosition(r).getCenter();
			
			LineSeg seg = new LineSeg(p1, p2);
			
			
			for(Obstacle o : map.getObstacles()){
				if(o.intersectsLine(seg))
					return false;
			}
				
			
			for(Robot r2 : robots){
				if(r != r2){
					if(config1.getRobotPosition(r2).intersectsLineSeg(seg))
						return false;
					if(config2.getRobotPosition(r2).intersectsLineSeg(seg))
						return false;
				}
			}
		}
		return true;
		
	}
}
